package astar;

import java.util.ArrayList;

import java.util.List;

import entity.Position;

public class AStar
{
	protected IAstarMap _Map;

	// private boolean diagonal;

	public AStar(boolean pDiagonal, IAstarMap pMap)
	{
		// this.diagonal = pDiagonal;
		_Map = pMap;
	}

	public AStar(IAstarMap pMap)
	{
		// this.diagonal = true;
		_Map = pMap;
	}

	public List<Position> searchPath( Position pStart , Position pFinish )
	{
		List<Position> result = new ArrayList<Position>();
		if ( pStart != null && pFinish != null )
		{
			Node start = new Node( pStart.getX(), pStart.getY(), 0.0 );
			Node finish = new Node( pFinish.getX(), pFinish.getY(), 0.0 );
			boolean succes = false;
			List<Node> listOpen = new ArrayList<Node>();
			List<Node> listClose = new ArrayList<Node>();
			listOpen.add( start );
			while ( !listOpen.isEmpty() && !succes )
			{
				Node current = getShort( listOpen );
				if ( pFinish.equals( current ) )
				{
					succes = true;
				}
				else
				{
					listOpen.remove( current );
					listClose.add( current );
					List<Node> successors = getSuccessor( current );
					for ( Node successor : successors )
					{
						Node isClose = isInList( listClose , successor );
						if ( !isObstacle( successor ) && isClose == null )
						{
							Node isOpen = isInList( listOpen , successor );
							if ( isOpen == null )
							{
								successor.setParent( current );
								successor.setDistanceDo( current
										.getDistanceDo()
										+ distance( successor , current ) );
								successor.setDistanceFly( distance( successor ,
										finish ) );
								listOpen.add( successor );
							}
							else
							{
								double distanceDo = current.getDistanceDo()
										+ distance( isOpen , current );
								if ( distanceDo < isOpen.getDistanceDo() )
								{
									isOpen.setDistanceDo( distanceDo );
									isOpen.setParent( current );
								}
							}
						}
					}
				}
			}
			if ( !listOpen.isEmpty() )
			{
				Node current = getShort( listOpen );
				while ( current != null )
				{
					result.add( current );
					current = current.getParent();
				}
			}
		}
		return result;
	}

	private List<Node> getSuccessor( Node pPos )
	{
		List<Node> result = new ArrayList<Node>();
		result.add( new Node( pPos.getX(), pPos.getY() + 1, pPos.getValue() + 1 ) );
		result.add( new Node( pPos.getX(), pPos.getY() - 1, pPos.getValue() + 1 ) );
		result.add( new Node( pPos.getX() + 1, pPos.getY(), pPos.getValue() + 1 ) );
		result.add( new Node( pPos.getX() - 1, pPos.getY(), pPos.getValue() + 1 ) );
		return result;
	}

	private Node getShort( List<Node> pList )
	{
		Node result = pList.get( 0 );
		for ( Node pos : pList )
		{
			if ( pos.getDistanceFly() < result.getDistanceFly() )
			{
				result = pos;
			}
		}
		return result;
	}

	private double distance( Position pStart , Position pFinish )
	{
		double valX = Math.pow( (pStart.getX() - pFinish.getX()) , 2 );
		double valY = Math.pow( (pStart.getY() - pFinish.getY()) , 2 );
		double valZ = Math.pow(
				_Map.getValue( pStart ) + _Map.getValue( pFinish ) , 2 );
		return Math.sqrt( valX + valY + valZ );
	}

	private Node isInList( List<Node> pList , Node pStart )
	{
		for ( Node node : pList )
		{
			if ( node.equals( pStart ) )
			{
				return node;
			}
		}
		return null;
	}

	private boolean isObstacle( Node pNode )
	{
		return _Map.isBlocked( pNode );
	}
}
